#ifndef FLASHROS_INFERENCE_INPUT_PIPELINE_H
#define FLASHROS_INFERENCE_INPUT_PIPELINE_H

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <spdlog/spdlog.h>

#include <chrono>
#include <mutex>
#include <opencv2/opencv.hpp>
#include <string>
#include <thread>

#include "flashros/commons/utils.h"

namespace flashros {
namespace inference {

class InputInterface {
 public:
  ~InputInterface() {}
  virtual bool Read(cv::Mat& frame) = 0;
  virtual void Release() = 0;
};

class VideoCaptureInput : public InputInterface {
 public:
  VideoCaptureInput() {}
  VideoCaptureInput(std::string input_video_path, ros::NodeHandle& nh) {
    Init(input_video_path);
  }

  void Init(std::string input_video_path) {
    cap_.open(input_video_path);
    SPDLOG_DEBUG("Try to open video {}", input_video_path);
    assert(cap_.isOpened());
    SPDLOG_INFO("Successfully Init VideoCaptureInput with {}",
                input_video_path);
  }

  bool Read(cv::Mat& frame) override {
    SPDLOG_INFO("Start read frame");
    bool flag = cap_.read(frame);
    SPDLOG_INFO("Successfully read frame ({}, {})", frame.rows, frame.cols);
    return flag;
  }

  void Release() override { cap_.release(); }

 private:
  cv::VideoCapture cap_;
};

class RosInput : public InputInterface {
 public:
  RosInput() {}
  RosInput(std::string camera_topic_name, int max_wait_secs,
           ros::NodeHandle& nh) {
    Init(camera_topic_name, max_wait_secs, nh);
  }

  void Init(std::string camera_topic_name, int max_wait_secs,
            ros::NodeHandle& nh);

  bool Read(cv::Mat& frame) override;

  void Release() override {
    running_ = false;
    for (auto& frame : frames_) frame.release();
    frames_.clear();
    SPDLOG_INFO("Ros input release");
  }

 private:
  std::thread thread_;
  std::mutex lock_;
  std::vector<cv::Mat> frames_;
  bool running_;
  int max_wait_secs_;
};

}  // namespace flashai
};  // namespace flashros

#endif  // FLASHROS_INFERENCE_INPUT_PIPELINE_H